/*
 * Copyright 2004 Program of Computer Graphics, Cornell University
 *     580 Rhodes Hall
 *     Cornell University
 *     Ithaca NY 14853
 * Web: http://www.graphics.cornell.edu/
 * 
 * Not for commercial use. Do not redistribute without permission.
 */
#include "stdafx.h"

#include "Sphere.h"

PrimitiveList *Sphere::compile(int num,const GeometryObject **array,PoolAllocator *pool) const
{
//_asm int 3; //force breakpoint
  PackedSphereList *list = PackedSphereList::poolAlloc(num,pool);
  for(int k=0;k<num;k++) {
    assert(array[k]->getType()==GEOM_SPHERE);
    const Sphere *t = (const Sphere*)array[k];
    list->setSphere(k,t->centerX,t->centerY,t->centerZ,t->uX,t->uY,t->uZ,t->wX,t->wY,t->wZ,
                        t->radius,t->id());
  }
  return list;
}

void Sphere::expandBoxToInclude(AxisAlignedBox *box) const 
{
  F32vec4 cent = getCenter();
  F32vec4 rad = F32vec4(radius);
  box->addPoint(cent-rad);
  box->addPoint(cent+rad);
}

AxisAlignedBox Sphere::getBoundingBox() const 
{
  F32vec4 cent = getCenter();
  F32vec4 rad = F32vec4(radius);
  return AxisAlignedBox(cent-rad,cent+rad);
}

bool Sphere::intersectsRay(const Ray &ray, IntersectionRecord *hitRec) const
{
  F32vec1 zero = F32vec1(0.0f);
  F32vec4 toc = ray.origin - getCenter();
  F32vec1 b = dotProduct(toc,ray.direction);
  F32vec1 c = dotProduct(toc,toc) - F32vec1(radius*radius);
  F32vec1 t1,t2;
  F32vec1 tStart = F32vec1(ray.tStart);
  F32vec1 tEnd = F32vec1(ray.tEnd);
  //solve for roots of equation t^2 + 2*b*t + c = 0
  //this version is more numerically stable than the straightforward application of the quadratic formula
  F32vec1 D = b*b - c;
  if (_mm_comilt_ss(D,zero))  // no solution
    return false;
  D = sqrt(D);
  if (_mm_comilt_ss(b,zero)) {
    t2 = D - b;
    t1 = c/t2;
  } else {
    t1 = zero - (D + b);
    t2 = c/t1;
  }
  if (_mm_comilt_ss(t1,tStart)) {
    t1 = t2;
  }
  if (_mm_comige_ss(t1,tStart)&&_mm_comilt_ss(t1,tEnd)) {
    F32vec1 one = F32vec1(1.0f);
    F32vec4 spt = toc + BroadcastFloat(t1)*ray.direction;
    F32vec4 uAxis = getUAxis();
    F32vec4 wAxis = getWAxis();
    spt = normalize3(spt);
    F32vec4 vAxis = crossProduct(wAxis,uAxis);
    F32vec1 costheta = dotProduct(spt,wAxis);
    F32vec1 invsintheta = rsqrt_nr(one - simd_min(one,costheta*costheta));
    F32vec1 cosphi = invsintheta*dotProduct(spt,uAxis);
    F32vec1 y = dotProduct(spt,vAxis);
    F32vec1 u = F32vec1(0.5f)*(one-costheta);
    F32vec1 v;
    if (_mm_comige_ss(y,zero)) {
      v = F32vec1(0.25f)*(one-cosphi);
    } else {
      v = F32vec1(0.25f)*(F32vec1(3.0f)+cosphi);
    }
    u = simd_min(one,simd_max(zero,u)); //clamp u to zero to one range in case of roundoff errors
    v = simd_min(one,simd_max(zero,v)); //clamp v to zero to one range in case of roundoff errors
    Store_F32vec1(&hitRec->t,t1);
    hitRec->primitiveId = id();
    Store_F32vec1(&hitRec->u,u);
    Store_F32vec1(&hitRec->v,v);
    hitRec->w = 0;
    return true;
  }
  return false;
}

bool Sphere::intersectsBox(const AxisAlignedBox &box) const
{
  //correctly treats the sphere as a hollow shell
  F32vec4 center = getCenter();
  F32vec4 zero = F32vec4(0.0f);
  //compute maximum and minimum separation from points in the box in each coordinate axis
  F32vec4 minDist = simd_max(zero,simd_max(box.min-center,center-box.max));
  F32vec4 maxDist = simd_max(box.max-center,center-box.min);
  F32vec1 minDist2 = dotProduct(minDist,minDist);
  F32vec1 maxDist2 = dotProduct(maxDist,maxDist);
  F32vec1 rad2 = F32vec1(radius*radius);
  if (_mm_comige_ss(maxDist2,rad2)&&_mm_comige_ss(rad2,minDist2)) {
    return true;
  } else {
    return false;
  }
}

AxisAlignedBox Sphere::estimateIntersectionBounds(const AxisAlignedBox &box) const
{
  //return a conservative bounding box by simply intersecting the input bounding box
  //and bounding box of the sphere.  This will often not be a tight bounding box for the
  //portion of the sphere that lies inside the input box.
  AxisAlignedBox bounds = getBoundingBox();
  bounds.min = simd_max(bounds.min,box.min);
  bounds.max = simd_min(bounds.max,box.max);
  return bounds;
}

void PackedSphereList::setSphere(int index, float centerX, float centerY, float centerZ, 
                                        float uX, float uY, float uZ,
                                        float wX, float wY, float wZ,
                                        float radius, int id)
{
  assert((length > index)&&(index>=0));
  int i = index;
  spheres[i].center = F32vec4(0,centerZ,centerY,centerX);
  spheres[i].uAxis = F32vec4(0,uZ,uY,uX);
  spheres[i].wAxis = F32vec4(0,wZ,wY,wX);
  spheres[i].radius = radius;
  spheres[i].id = id;
}

/*
void IntersectPackedSpheres(class PrimitiveList *list, class Ray *ray, class IntersectionRecord *hitRec)
{
  F32vec1 zero = F32vec1(0.0f);
  F32vec1 tStart = F32vec1(ray->tStart);
  F32vec1 tEnd = F32vec1(ray->tEnd);
  PackedSphere *spheres = ((PackedSphereList*)list)->spheres;
  for(int count = list->length;count>0;count-=1,spheres++) {
    F32vec4 toc = ray->origin - spheres->center;
    F32vec1 b = dotProduct(toc,ray->direction);
    F32vec1 c = dotProduct(toc,toc) - F32vec1(spheres->radius*spheres->radius);
    F32vec1 t1,t2;
    //solve for roots of equation t^2 + 2*b*t + c = 0
    //this version is more numerically stable than the straightforward application of the quadratic formula
    F32vec1 D = b*b - c;
    if (_mm_comilt_ss(D,zero))  // no solution
      continue;
    D = sqrt(D);
    if (_mm_comilt_ss(b,zero)) {
      t2 = D - b;
      t1 = c/t2;
    } else {
      t1 = zero - (D + b);
      t2 = c/t1;
    }
    if (_mm_comilt_ss(t1,tStart)) {
      t1 = t2;
    }
    if (_mm_comige_ss(t1,tStart)&&_mm_comilt_ss(t1,tEnd)) {
//_asm int 3; //force debugger breakpoint
      F32vec1 one = F32vec1(1.0f);
      F32vec4 spt = toc + BroadcastFloat(t1)*ray->direction;
      F32vec4 uAxis = spheres->uAxis;
      F32vec4 wAxis = spheres->wAxis;
      F32vec4 vAxis = crossProduct(wAxis,uAxis);
      F32vec1 z = dotProduct(spt,wAxis);
      F32vec1 x = dotProduct(spt,uAxis);
      F32vec1 y = dotProduct(spt,vAxis);
      F32vec1 costheta = z * rsqrt(x*x + y*y + z*z);
      F32vec1 u = F32vec1(0.5f)*(one-costheta);
//      F32vec1 cosphi = F32vec1(x) * rsqrt(F32vec1(x*x + y*y));
      F32vec1 v;
      if (_mm_comige_ss(x*x,y*y)) { //which component has the greater absolute value?
        if (_mm_comige_ss(x,zero)) {
          v = F32vec1(0.125f)*(one + y/x);
        } else {
          v = F32vec1(0.125f)*(F32vec1(5.0f) + y/x);
        }
      } else {
        if (_mm_comige_ss(y,zero)) {
          v = F32vec1(0.125f)*(F32vec1(3.0) - x/y);
        } else {
          v = F32vec1(0.125f)*(F32vec1(7.0) - x/y);
        }
      }
      u = simd_min(one,simd_max(zero,u)); //clamp u to zero to one range in case of roundoff errors
      v = simd_min(one,simd_max(zero,v)); //clamp v to zero to one range in case of roundoff errors
      if (hitRec->func) { //if we have a custom intersection responder, then use it
        (*hitRec->func)(ray,hitRec,F32vec4(t1)[0],F32vec4(u)[0],F32vec4(v)[0],0,spheres->id);
        tEnd = F32vec1(ray->tEnd); //reload in case it has changed
      } else { //otherwise the default is find first intersection
        hitRec->t = ray->tEnd = F32vec4(t1)[0];
        hitRec->u = F32vec4(u)[0];
        hitRec->v = F32vec4(v)[0];
        hitRec->w = 0.0f;
        hitRec->primitiveId = spheres->id;
        tEnd = t1;
      }
    }
  }
}
*/
//unfortunately the SSE packed float version had numerical problems near the seams in
//the primitive uv mapping.  This all double version seems to solve those problems
void IntersectPackedSpheres(class PrimitiveList *list, class Ray *ray, class IntersectionRecord *hitRec)
{
  double tStart = ray->tStart;
  double tEnd = ray->tEnd;
  PackedSphere *spheres = ((PackedSphereList*)list)->spheres;
  for(int count = list->length;count>0;count-=1,spheres++) {
    double TOCx = ray->originX() - (double)(spheres->center[0]);
    double TOCy = ray->originY() - (double)(spheres->center[1]);
    double TOCz = ray->originZ() - (double)(spheres->center[2]);
    double b = TOCx*ray->dirX() + TOCy*ray->dirY() + TOCz*ray->dirZ();
    double c = TOCx*TOCx + TOCy*TOCy + TOCz*TOCz - spheres->radius*spheres->radius;
    double t1,t2;
    //solve for roots of equation t^2 + 2*b*t + c = 0
    //this version is more numerically stable than the straightforward application of the quadratic formula
    double D = b*b - c;
    if (D < 0)   // no solution
      continue;
    D = sqrt(D);
    if (b < 0) {
      t2 = D - b;
      t1 = c/t2;
    } else {
      t1 = -(D + b);
      t2 = c/t1;
    }
    if (t1 < tStart) {
      t1 = t2;
    }
    if ((t1>=tStart)&&(t1<tEnd)) {
//_asm int 3; //force debugger breakpoint
      double t = t1;
      double ptx = ray->origin[0] + ray->direction[0]*t - spheres->center[0];
      double pty = ray->origin[1] + ray->direction[1]*t - spheres->center[1];
      double ptz = ray->origin[2] + ray->direction[2]*t - spheres->center[2];
      F32vec4 uAxis = spheres->uAxis;
      F32vec4 wAxis = spheres->wAxis;
      F32vec4 vAxis = crossProduct(wAxis,uAxis);
      double z = ptx*wAxis[0] + pty*wAxis[1] + ptz*wAxis[2];
      double x = ptx*uAxis[0] + pty*uAxis[1] + ptz*uAxis[2];
      double y = ptx*vAxis[0] + pty*vAxis[1] + ptz*vAxis[2];
      double costheta = z / sqrt(x*x + y*y + z*z);
      double u = 0.5*(1.0-costheta);
      double v;
      if (x*x >= y*y) { //see which has the greater absolute magnitude
        if (x >= 0) {
          v = 0.125*(1 + y/x);
        } else {
          v = 0.125*(5 + y/x);
        }
      } else {
        if (y >= 0) {
          v = 0.125*(3 - x/y);
        } else {
          v = 0.125*(7 - x/y);
        }
      }
      if (u<0) u = 0;
      if (u>1) u = 1;
      if (v<0) v = 0;
      if (v>1) v = 1;
      if (hitRec->func) { //if we have a custom intersection responder, then use it
        (*hitRec->func)(ray,hitRec,
            static_cast<float>(t), static_cast<float>(u), static_cast<float>(v),
            0,spheres->id);
        tEnd = ray->tEnd; //reload in case it has changed
      } else { //otherwise the default is find first intersection
        hitRec->t = ray->tEnd = static_cast<float>(t);
        hitRec->u = static_cast<float>(u);
        hitRec->v = static_cast<float>(v);
        hitRec->w = 0.0f;
        hitRec->primitiveId = spheres->id;
        tEnd = t;
      }
    }
  }
}

